#include "Chassis.h"
#include "FastMathFunctions.h"

/* 底盘初始化 */
inline void Chassis_Init(void)
{
    AttachInterrupt_CAN(&hcan1, DJ_CAN_Callback); // 连接大疆电机CAN中断回调

    /* 码盘初始化 */
    Action_Init();

    /* 初始化RMDS电机驱动器 */
    for (uint8_t i = 0; i < 4; i++)
    {
        DJ_Init(&chassis.ChassisMotors[i], i + 1, M3508, PID_METHOD);
    }

    /* 底盘x坐标pid初始化 */
    TDPID_Init(&chassis.PID_X, 6.0, 0.01f, 0.9f, 3000.0f, 3.0f, 0.003f, 1000.0f);
    /* 底盘y坐标pid初始化 */
    TDPID_Init(&chassis.PID_Y, 6.0, 0.01f, 0.9f, 3000.0f, 3.0f, 0.003f, 1000.0f);
    /* 底盘角度pid初始化 */
    TDPID_Init(&chassis.PID_Angle, 2.0, 0.01f, 3.0f, 3.0f, 0.1f, 0.003f, 10.0f);
}

/**
 * 底盘运行
 * 需要循环运行
 */
inline void Chassis_Run(void)
{
    if (chassis.ctrlMode == VELOCITY_MODE)
    {
        /* 合速度大小 */
        float V = sqrtf(chassis.setVx * chassis.setVx + chassis.setVy * chassis.setVy);
        /* 合速度与Y轴夹角 */
        float angle = atan2f(chassis.setVx, chassis.setVy) + RAD(Action.yaw);
        /* Vx */
        float Vx = V * fast_sin(angle);
        /* Vy */
        float Vy = V * fast_cos(angle);
        /* 底盘电机速度计算 */
        chassis.Motors_Speed[0] = (-V * Vx + V * Vy + chassis.setVw * LEGNTH_WIDTH) * COEFFICIENT;
        chassis.Motors_Speed[1] = (-V * Vx - V * Vy + chassis.setVw * LEGNTH_WIDTH) * COEFFICIENT;
        chassis.Motors_Speed[2] = (V * Vx - V * Vy + chassis.setVw * LEGNTH_WIDTH) * COEFFICIENT;
        chassis.Motors_Speed[3] = (V * Vx + V * Vy + chassis.setVw * LEGNTH_WIDTH) * COEFFICIENT;
        for (uint8_t i = 0; i < 4; i++)
        {
            DJ_SetSpeed(&chassis.ChassisMotors[i], chassis.Motors_Speed[i]);
        }
    }
    else if (chassis.ctrlMode == POSITION_MODE)
    {
        /* 位置PID计算x轴速度, y轴速度, 自转角速度 */
        TDPID_Calc(&chassis.PID_X, Action.pos_x - OFFSET * fast_sin(-RAD(Action.yaw)), chassis.setPx);
        TDPID_Calc(&chassis.PID_Y, Action.pos_y - OFFSET * (fast_cos(RAD(Action.yaw)) - 1), chassis.setPy);
        TDPID_Calc(&chassis.PID_Angle, RAD(Action.yaw), chassis.setYaw);

        /* 合速度大小 */
        float V = sqrtf(chassis.PID_X.out * chassis.PID_X.out + chassis.PID_Y.out * chassis.PID_Y.out);
        /* 合速度与Y轴夹角 */
        float angle = atan2f(chassis.PID_X.out, chassis.PID_Y.out) + RAD(Action.yaw);
        /* Vx */
        float Vx = V * fast_sin(angle);
        /* Vy */
        float Vy = V * fast_cos(angle);
        /* 底盘电机速度计算 */
        chassis.Motors_Speed[0] = (-V * Vx + V * Vy - chassis.PID_Angle.out * LEGNTH_WIDTH) * COEFFICIENT;
        chassis.Motors_Speed[1] = (-V * Vx - V * Vy - chassis.PID_Angle.out * LEGNTH_WIDTH) * COEFFICIENT;
        chassis.Motors_Speed[2] = (V * Vx - V * Vy - chassis.PID_Angle.out * LEGNTH_WIDTH) * COEFFICIENT;
        chassis.Motors_Speed[3] = (V * Vx + V * Vy - chassis.PID_Angle.out * LEGNTH_WIDTH) * COEFFICIENT;
        for (uint8_t i = 0; i < 4; i++)
        {
            DJ_SetSpeed(&chassis.ChassisMotors[i], chassis.Motors_Speed[i]);
        }
    }

#if USE_CAN
    /* 大疆电机运行 */
    DJ_MotorRun();
#endif
}

/**
 * 设置底盘速度
 * 单位:mm/s
 * 单位:rad/s
 */
inline void Chassis_SetVel(float Vx, float Vy, float Vw)
{
    chassis.ctrlMode = VELOCITY_MODE;
    chassis.setVx = Vx;
    chassis.setVy = Vy;
    chassis.setVw = Vw;
}

/**
 * 设置底盘位置
 * 单位:mm
 * 单位:rad
 */
inline void Chassis_SetPos(float Px, float Py, float Yaw)
{
    chassis.ctrlMode = POSITION_MODE;
    chassis.setPx = Px;
    chassis.setPy = Py;
    chassis.setYaw = Yaw;
}
